#include <tsubamegaeshi-base/worldmodel.h>

#include <cmath>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;
using namespace boost::numeric;

typedef ublas::vector<double> dvector;

Worldmodel::TVector3 polar2xyz(Worldmodel::TVector3 pol)
{
  return Worldmodel::TVector3
    (pol.v_[0] * cos(pol.v_[2] * M_PI / 180) * cos(pol.v_[1] * M_PI / 180),
     pol.v_[0] * cos(pol.v_[2] * M_PI / 180) * sin(pol.v_[1] * M_PI / 180),
     pol.v_[0] * sin(pol.v_[2] * M_PI / 180));
}

void
Worldmodel::TStatus::calc_position()
{
  Worldmodel::TVector3 f1l = flag_["F1L"];
  Worldmodel::TVector3 f1r = flag_["F1R"];
  Worldmodel::TVector3 f2l = flag_["F2L"];
  
  Worldmodel::TVector3 tmp = (polar2xyz(f1l));
  dvector f1l_xyz(3);
  f1l_xyz[0] = tmp.v_[0];
  f1l_xyz[1] = tmp.v_[1];
  f1l_xyz[2] = tmp.v_[2];

  tmp = (polar2xyz(f1r));
  dvector f1r_xyz(3);
  f1r_xyz[0] = tmp.v_[0];
  f1r_xyz[1] = tmp.v_[1];
  f1r_xyz[2] = tmp.v_[2];

  tmp = (polar2xyz(f2l));
  dvector f2l_xyz(3);
  f2l_xyz[0] = tmp.v_[0];
  f2l_xyz[1] = tmp.v_[1];
  f2l_xyz[2] = tmp.v_[2];

  dvector xax(3);
  xax = f1r_xyz - f1l_xyz ;
  double l = norm_2(xax);
  xax /= l;

  dvector yax(3);
  yax = f1l_xyz - f2l_xyz;
  double w = norm_2(yax);
  yax /= w;

  matrix<double> op(3,3);
  op = outer_prod( xax, yax); 

  dvector zax(3);
  zax[0] = op(1,2) - op(2,1);
  zax[1] = op(2,0) - op(0,2);
  zax[2] = op(0,1) - op(1,0);
  zax = zax / norm_2(zax);

  zax_ = Worldmodel::TVector3(zax[0], zax[1], zax[2]);
  double r1 = f1l.v_[0];
  double r2 = f1r.v_[0];
  double r3 = f2l.v_[0];

  double x = (l*l + r1*r1 - r2*r2) / ( 2 * l);
  double y = (w*w - r1*r1 + r3*r3) / ( 2 * w);
  double z = norm_2(f2l_xyz + x * xax + y * yax);

  x = x - (l / 2);
  y = y - (w / 2);

  position_ = Worldmodel::TVector3(x, y, z);
}
